#include "camshift.hpp"

Cam_Shift::Cam_Shift(Process *pro, States *_state):Observer(pro)
{
	state = _state;
	histimg = Mat::zeros(200, 320, CV_8UC3);
	hsize = 16;
	hranges[0] = 0;hranges[1] = 180;
	phranges = hranges;
	vmin = 10;
	vmax = 256;
	smin = 50;
}

Cam_Shift::~Cam_Shift()
{
	Log("Release CamShift");
	
	_pro->Detach(this);
}

void Cam_Shift::GetMask(Image *_mask)
{
	mask = Mat(_mask->height, _mask->width, CV_8UC1, _mask->data);
}

void Cam_Shift::Update(Image &image)
{
	cvtColor(Mat(image.height, image.width, CV_8UC3, image.data), hsv, CV_BGR2HSV);

	if( state->TrackObject )
	{
		int _vmin = vmin, _vmax = vmax;

		inRange(hsv, Scalar(0, smin, MIN(_vmin,_vmax)), Scalar(180, 256, MAX(_vmin, _vmax)), mask);
		int ch[] = {0, 0};
		hue.create(hsv.size(), hsv.depth());
		mixChannels(&hsv, 1, &hue, 1, ch, 1);

		if( state->TrackObject < 0 )
		{
			Mat roi(hue, Rect(state->selection.x, state->selection.y, state->selection.width, state->selection.height)), maskroi(mask, Rect(state->selection.x, state->selection.y, state->selection.width, state->selection.height));
			calcHist(&roi, 1, 0, maskroi, hist, 1, &hsize, &phranges);
			normalize(hist, hist, 0, 255, CV_MINMAX);
			
			trackWindow = Rect(state->selection.x, state->selection.y, state->selection.width, state->selection.height);
			state->TrackObject = 1;

			histimg = Scalar::all(0);
			int binW = histimg.cols / hsize;
			Mat buf(1, hsize, CV_8UC3);
			for( int i = 0; i < hsize; i++ )
				buf.at<Vec3b>(i) = Vec3b(saturate_cast<uchar>(i*180./hsize), 255, 255);
			cvtColor(buf, buf, CV_HSV2BGR);
				
			for( int i = 0; i < hsize; i++ )
			{
				int val = saturate_cast<int>(hist.at<float>(i)*histimg.rows/255);
				rectangle( histimg, Point(i*binW,histimg.rows),
						   Point((i+1)*binW,histimg.rows - val),
						   Scalar(buf.at<Vec3b>(i)), -1, 8 );
			}
		}
		
		calcBackProject(&hue, 1, 0, hist, backproj, &phranges);
		backproj &= mask;
		CamShift(backproj, trackWindow, TermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ));
		if( trackWindow.area() <= 1 )
		{
			int cols = backproj.cols, rows = backproj.rows, r = (MIN(cols, rows) + 5)/6;
			trackWindow = Rect(trackWindow.x - r, trackWindow.y - r,
							   trackWindow.x + r, trackWindow.y + r) &
						  Rect(0, 0, cols, rows);
		}
		
		TrackBox = I_Rect(trackWindow.x , trackWindow.y , trackWindow.width , trackWindow.height);
	}
}
